
import os
import numpy as np
import cv2
import glob
import matchfunc
from miscfunc import *

##################################
##相机内参
K=np.array([[1.04874513e+03,0.00000000e+00,6.60010650e+02],
   [0.00000000e+00,1.03569261e+03,3.76745246e+02],
   [0.00000000e+00,0.00000000e+00,1.00000000e+00]])
Distortion=np.array([[ 3.49842462e-01, -2.72871065e+00, -2.16643670e-04, -5.38428370e-03, 5.69940149e+00]])

##################################
matches_matrix={}       #dict, (i,j)==>matches 图像相互间的点匹配，原始
matches_matrix_good={}  #dict, (i,j)==>matches 图像相互间的点匹配，经F-修剪后的
F_matrix={}
homo_ratio=[]           #[i,j,ration],  (i,j)==>ratio    保存图像两两间的homography程度，百分比
descs=[]                #列表，每个图像的特征descriptor
keypts=[]               #列表，特征的keypoint
imgs=[]                 #列表，图像数据

######1，给定目录，读取图像文件名，编号为0,1,2,...
path="e:/"
files=glob.glob(r'f:\scene\out*.jpg')
#fileList=read_dir_files(path)
N=len(files)     #总图像数量
#print(fileList)

######2，读取图像内容，对各图像进行特征点检测
#surf = cv2.xfeatures2d.SURF_create(4000)  # 参数为Hessian阀值
sift = cv2.xfeatures2d.SIFT_create()

for file in files:
    img = cv2.imread(file)
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    imgs.append(gray)
    #kp, des = surf.detectAndCompute(gray,None)
    kp,des=sift.detectAndCompute(gray,None)
    descs.append(des)
    keypts.append(kp)

######3，图像间两两进行特征点匹配
for i in range(N-1):
    for j in range(i+1,N):
        matches=matchfunc.match_flann(descs[i],descs[j])     #进行flann匹配
        ##对称匹配处理
        matches2=[]
        for match in matches:
            match_tmp=cv2.DMatch()
            match_tmp.distance=match.distance
            match_tmp.trainIdx=match.queryIdx
            match_tmp.queryIdx=match.trainIdx
            match_tmp.imgIdx=match.imgIdx
            matches2.append(match_tmp)
        matches_matrix[(i, j)] = matches
        matches_matrix[(j, i)] = matches2
        #display
        disp_matches(imgs[i], imgs[j], keypts[i], keypts[j], matches)

print("match done!")

######4，计算图像相互间的F矩阵，并据此修剪点匹配
for i in range(N-1):
    for j in range(i+1,N):
        F_ij,goodMatches_ij=matchfunc.computeFandPruneMatches(keypts[i],keypts[j],matches_matrix[(i,j)])
        F_ji,goodMatches_ji=matchfunc.computeFandPruneMatches(keypts[j],keypts[i],matches_matrix[(j,i)])
        matches_matrix_good[(i, j)] = goodMatches_ij
        matches_matrix_good[(j, i)] = goodMatches_ji
        F_matrix[(i,j)]=F_ij
        F_matrix[(j,i)]=F_ji
        #display
        #disp_matches(imgs[i], imgs[j], keypts[i], keypts[j], goodMatches_ij)

######5，计算图像两两间的homography，并对homography的程度升序排序
for (imagePair,matches) in matches_matrix_good.items():
    (i,j)=imagePair
    if len(matches)<100:        #如果匹配点对的数量不足，则直接设为100
        homo_ratio.append([i,j,100])
    else:
        #计算两图像间的homography
        N=len(matches_matrix[imagePair])
        pts1,pts2=matchfunc.fetchCoordinate(keypts[i],keypts[j],matches_matrix[imagePair])
        H,mask= cv2.findHomography(pts1, pts2, cv2.RANSAC, 5.0)
        homo_ratio.append([i,j,int(sum(mask.ravel())/N*100.0)])     #[i,j,ratio]

homo_ratio.sort(key=lambda x:x[2])      #按ratio升序排序
print(homo_ratio)
######6,按homography比率从小到大，依次取图像对，计算图像对的相机矩阵及重建3D点，止到取到适合的为止，作为初始的3D重建
hasGoodF=False
for i,j,ratio in homo_ratio:
    F=F_matrix[(i,j)]
    pts1, pts2 = matchfunc.fetchCoordinate(keypts[i], keypts[j], matches_matrix_good[(i,j)])     ##提取出两图像间满足F矩阵的匹配对点
    isGood,P,Pp,pts3D=matchfunc.computeCameraMatrix(pts1,pts2,F,K)         #计算相机矩阵，并得到重建3D点
    if isGood:
        hasGoodF=True
        break
print(Pp)
######7,
